#include "roboticArmCtrl.h"
#include "logger.h"

#include <QFileInfo>
#include <QSettings>
#include <QTime>
#include <QDir>

#include <time.h>
#include <rapidjson/document.h>
#include <rapidjson/stringbuffer.h>
#include <rapidjson/prettywriter.h>

roboticArmCtrl::roboticArmCtrl()
{
    loadINI();
    pingger.init(robotIP);
}

roboticArmCtrl::~roboticArmCtrl()
{
    elite_robot.disconnect();
}

void roboticArmCtrl::print_base_info()
{
    LOG(INFO) << "roboticArm IP: "    << robotIP     << std::endl;
    LOG(INFO) << "roboticArm Port: "  << robotPort   << std::endl;
    LOG(INFO) << "roboticArm Speed: " << robot_speed << std::endl;
}

void roboticArmCtrl::loadINI()
{
    QFileInfo fileInfo(CONFIG_FILE_PATH);
    if(fileInfo.isFile())
    {
        QSettings setting(CONFIG_FILE_PATH, QSettings::IniFormat);
        setting.setIniCodec("utf-8");

        robot_speed  = setting.value("/ARMS/robotSpeedPercent").toInt();
        std::string ip = setting.value("/ARMS/robotIP").toString().toStdString();
        memset(robotIP, 0, sizeof (robotIP));
        sprintf(robotIP, "%s", ip.c_str());
    }
}

bool roboticArmCtrl::connectArm()
{
    if(true == mInited){
        return true;
    }

    print_base_info();
    elite_robot.init(robotIP, robotPort);

    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    if(!elite_robot.isConnected())
    {
        LOG(ERROR) << "elite_robot connect failed!!!\n";
        return false;
    }
    mInited = true;
    return mInited;
}

int roboticArmCtrl::set_servo_on(bool& reslut)
{

    int nRet = elite_robot.getServoStatus(reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "getServoStatus nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }
    if(reslut){
        return ELITE_SDK_PARSE_JSON_OK;
    }

    nRet = elite_robot.clearAlarm(reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet || !reslut){
        LOG(ERROR) << "clearAlarm nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }

    //clearAlarm 需要时间，getMotorStatus轮询
    int times =0;
    reslut = false;
    while(!reslut&&times<50){
        nRet = elite_robot.getMotorStatus(reslut);
        if(ELITE_SDK_PARSE_JSON_OK != nRet){
            LOG(ERROR) << "getMotorStatus nRet = " << nRet <<" err!" <<std::endl;
            return nRet;
        }
        if(!reslut){
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        times++;
    }

    if(times>50){
        LOG(ERROR) << "getMotorStatus out of time!" <<std::endl;
        reslut = false;
        return ELITE_SDK_PARSE_JSON_OK;
    }


    nRet = elite_robot.syncMotorStatus(reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet || !reslut){
        LOG(ERROR) << "syncMotorStatus nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }

    nRet = elite_robot.set_servo_status(ELITE_SDK_ENABLE,reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet || !reslut){
        LOG(ERROR) << "set servo status nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }
    return ELITE_SDK_PARSE_JSON_OK;
}

int roboticArmCtrl::set_servo_off(bool &reslut)
{
    int nRet = elite_robot.getServoStatus(reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "getServoStatus nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }
    if(!reslut){
        return ELITE_SDK_PARSE_JSON_OK;
    }

    nRet = elite_robot.set_servo_status(ELITE_SDK_DISABLE,reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet || !reslut){
        LOG(ERROR) << "set servo status nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }
    return ELITE_SDK_PARSE_JSON_OK;
}
bool roboticArmCtrl::is_connected()
{
    return elite_robot.isConnected();
}

int roboticArmCtrl::stop(bool& optRet)
{
    int nRet = elite_robot.stop(optRet);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "stop nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }else{
        LOG(INFO) << "optRet = " << optRet <<std::endl;
    }
    return ELITE_SDK_PARSE_JSON_OK;
}

int roboticArmCtrl::setSpeed(double speed_value, bool &reslut)
{
    int nRet = elite_robot.setSpeed(speed_value,reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "setSpeed nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }else{
        LOG(INFO) << "reslut = " << reslut <<std::endl;
    }
    return ELITE_SDK_PARSE_JSON_OK;
}

int roboticArmCtrl::runJbi(std::string filename, bool &reslut)
{
    int nRet = elite_robot.runJbi(filename,reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "runJbi nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }else{
        LOG(INFO) <<  "reslut = " << reslut <<std::endl;
    }
    return ELITE_SDK_PARSE_JSON_OK;
}

int roboticArmCtrl::setCollisionEnable(ELITE_SDK_SWTICH col_switch, bool &reslut)
{
    int nRet = elite_robot.setCollisionEnable(col_switch,reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "runJbi nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }else{
        LOG(INFO) <<  "reslut = " << reslut <<std::endl;
    }
    return ELITE_SDK_PARSE_JSON_OK;
}

int roboticArmCtrl::jbi_arm_move_out_box(bool &reslut, int timeoutMS)
{
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_MOVE_OUT_BOX, timeoutMS);
}

int roboticArmCtrl::jbi_arm_move_back_box(bool &reslut, int timeoutMS)
{
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_MOVE_BACK_BOX, timeoutMS);
}

int roboticArmCtrl::jbi_arm_charge(bool &reslut, int side, int timeoutMS)
{
    //设置参数1
    int nRet = elite_robot.setSysVarI(ARM_JBI_PARAM1_IADDR, side, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "setSysVarI nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_CHARGE, timeoutMS);
}

int roboticArmCtrl::jbi_arm_draw(bool &reslut, int side, int timeoutMS)
{
    //设置参数1
    int nRet = elite_robot.setSysVarI(ARM_JBI_PARAM1_IADDR, side, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "setSysVarI nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_DRAW, timeoutMS);
}

int roboticArmCtrl::jbi_arm_shot(bool &reslut, int side, int timeoutMS)
{
    //设置参数1
    int nRet = elite_robot.setSysVarI(ARM_JBI_PARAM1_IADDR, side, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "setSysVarI nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_SHOT, timeoutMS);
}

int roboticArmCtrl::jbi_arm_move_offset(bool &reslut, double offsetX, double offsetY, double offsetZ, int timeoutMS)
{
    //设置参数offsetX
    int nRet = elite_robot.setSysVarD(ARM_JBI_X_OFFSET_DADDR, offsetX, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "setSysVarI nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }
    //设置参数offsetY
    nRet = elite_robot.setSysVarD(ARM_JBI_Y_OFFSET_DADDR, offsetY, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "setSysVarI nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }
    //设置参数offsetZ
    nRet = elite_robot.setSysVarD(ARM_JBI_Z_OFFSET_DADDR, offsetZ, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "setSysVarI nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_MOVE_OFFSET, timeoutMS);
}

int roboticArmCtrl::jbi_arm_restart_lua(bool &reslut, int timeoutMS)
{
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_RESTART_LUA, timeoutMS);
}

int roboticArmCtrl::jbi_arm_fixed_point_charge(bool &reslut, int timeoutMS)
{
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_FIXED_POINT_CHARGE, timeoutMS);
}

int roboticArmCtrl::jbi_arm_fixed_point_draw(bool &reslut, int timeoutMS)
{
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_FIXED_POINT_DRAW, timeoutMS);
}

int roboticArmCtrl::jbi_arm_move_charge_center(bool &reslut, int timeoutMS)
{
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_MOVE_CHARGE_CENTER, timeoutMS);
}

int roboticArmCtrl::jbi_arm_move_failed_back_before(bool &reslut, int timeoutMS)
{
    return jbi_arm_send_cmd_get_reslut(reslut, ARM_JBI_CMD_MOVE_FAILED_BACK_BEFORE, timeoutMS);
}

int roboticArmCtrl::jbi_arm_send_cmd_get_reslut(bool& reslut, int cmd, int timeoutMS)
{
    //1、清返回值寄存器
    int value = ARM_JBI_NONE;
    int nRet = elite_robot.setSysVarI(ARM_JBI_RET_IADDR, value, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "setSysVarI nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }

    if(false == reslut){
        LOG(ERROR) << "setSysVarI " << ARM_JBI_PARAM1_IADDR <<" = " <<ARM_JBI_NOT_DONE << " failed!" <<std::endl;
        return ELITE_SDK_SET_SYS_VALUE_ERR;
    }

    //2、发送命令
    nRet = elite_robot.setSysVarI(ARM_JBI_CMD_IADDR, cmd, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "setSysVarI nRet = " << nRet <<" err!" <<std::endl;
        return nRet;
    }

    if(false == reslut){
        LOG(ERROR) << "setSysVarI " << ARM_JBI_CMD_IADDR <<" = " <<cmd << " failed!" <<std::endl;
        return ELITE_SDK_SET_SYS_VALUE_ERR;
    }

    //3、轮询等待完成
    int i;
    int errnum=0;
    for(i=0; i<timeoutMS; i+=100){
        nRet = elite_robot.getSysVarI(ARM_JBI_RET_IADDR, value);
        if(ELITE_SDK_PARSE_JSON_OK != nRet){
            errnum++;
            if(errnum>5){
                LOG(ERROR) << "setSysVarI nRet = " << nRet <<" err!" <<std::endl;
                return nRet;
            }else{
                LOG(ERROR) << "setSysVarI nRet = " << nRet <<" err!" <<std::endl;
                LOG(ERROR) << "errnum = " << errnum <<" continue!" <<std::endl;
                std::this_thread::sleep_for(std::chrono::milliseconds(500));
                continue;
            }

        }

        errnum =0;
        LOG(INFO) << "getSysVarI value = " <<value<<std::endl;
        if(value == ARM_JBI_NONE){
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }else{
            break;
        }
    }

    if(i>=timeoutMS){
        reslut = false;
        LOG(ERROR) << "do cmd = " << cmd <<" timeout! timeoutMS " << timeoutMS<<std::endl;
    }else{
        if(value == ARM_JBI_DONE){
            reslut = true;
        }else{
            reslut = false;
        }
    }

    return ELITE_SDK_PARSE_JSON_OK;
}
